#include "InterceptBallBehaviour.h"
#include "../CommonFunctions.h"


/*
 *	Zachyti loptu ak je to mozne. Vracia false ak to nie je mozne.
 *
 *	
 */
bool InterceptBallBehaviour::Behave()
{ 	
	if(!DATA_STORAGE.DependentForBall())
	return false;

	PlayMode pm = world->state->GetPlayMode();
	// Neminaj si zbytocne energiu, ked zahrava super
	if(pm == PM_GoalKick_Their || pm == PM_FreeKick_Their || pm == PM_KickOff_Their || pm == PM_KickIn_Their || pm == PM_CornerKick_Their)
		return false;

	if(DATA_STORAGE.ballStates[DATA_STORAGE.actualBallStateIndex] != NULL)
	{
		world->ball->pos = DATA_STORAGE.ballStates[DATA_STORAGE.actualBallStateIndex]->pos;
		world->ball->vel = DATA_STORAGE.ballStates[DATA_STORAGE.actualBallStateIndex]->vel;
	}

	Point pointInterception = GetBallInterceptionPoint();

	// Optimalne miesto, kde zachytime loptu
	RunToBallInteceptPosition(pointInterception);
	return true;	
}

// Vypocita optimalny bod pre zachytenie lopty
Point InterceptBallBehaviour::GetBallInterceptionPoint()
{
	const int MAX_STEPS = 14; // 2^(14-2) = 4096 > 3000	
	  
	double gamma[MAX_STEPS];
	int time[MAX_STEPS];
	
	int nResTime;
	Point pointResPosition;

	time[0]		= 0;
	gamma[0]	= 1.0;
	
	time[1] = 1;
	gamma[1] = world->params->server->ball_decay;
	
	if (IsBallInPlayerKickableAreaDuringInteception(gamma[0], time[0]))
	{
		nResTime = time[0];
		pointResPosition = world->ball->pos;
		return pointResPosition;
	} 

	if (IsBallInPlayerKickableAreaDuringInteception(gamma[1], time[1])) 
	{
		nResTime = time[1];
		pointResPosition = world->ball->pos + world->ball->vel;
		return pointResPosition;
	}

	int i = 2;
  
	while(1)
	{
		gamma[i] = gamma[i-1] * gamma[i-1];
		time[i] = time[i-1] + time[i-1];
	  
		if (IsBallInPlayerKickableAreaDuringInteception(gamma[i], time[i])) 
			break;
		i++;

		if ( i >= MAX_STEPS )
		{
			// Potrebujem viac ako time[i-1] krokov 
			nResTime= time[i-1] ;
			pointResPosition =  world->ball->pos +  world->ball->vel * 
				(1.0f - (float)gamma[i-1]) / ( 1.0f - world->params->server->ball_decay);
			return pointResPosition;
		}
	}
	
	i = i - 1;
  
	int nMinTime = time[i];
	int nMidTime = time[i] + time[i-1];
	int nMaxTime = time[i+1];
	double lfMinGamma = gamma[i];
	double lfMidGamma = gamma[i] * gamma[i-1];
	double lfMaxGamma = gamma[i+1];

	while (i > 1)
	{
		if (IsBallInPlayerKickableAreaDuringInteception(lfMidGamma, nMidTime))
		{
		  lfMaxGamma = lfMidGamma;
		  lfMidGamma = lfMinGamma * gamma[i-2];
		  nMaxTime = nMidTime;
		  nMidTime = nMinTime + time[i-2];
		}
		else
		{
		  lfMinGamma = lfMidGamma;
		  lfMidGamma = lfMinGamma * gamma[i-2];
		  nMinTime = nMidTime;
		  nMidTime = nMinTime + time[i-2];
		}
 
		i= i-1;
	}

	nResTime = nMaxTime;  
	pointResPosition = world->ball->pos + world->ball->vel * 
		(1.0f - (float)lfMaxGamma)/(1.0f - world->params->server->ball_decay);

/* ----------------------- LOG ----------------------- //
	char buffer[256];
	sprintf(buffer, "!TXT Interception\\%d", world->me->GetUniform());
	ofstream log1(buffer, ios::app);
	int nSimTime = world->state->GetSimTime();


	log1 << "Vysledny cas: " << nResTime << endl
	<< "Prerusovaci bod: (" << pointResPosition.GetX() << ", " << pointResPosition.GetY() << ")" << endl;
	log1.close();
// ----------------------- // LOG ----------------------- */
	
	return pointResPosition;	
}


// Zisti, ci lopta je v danom cykle na dosah hraca
// Validne hodnoty pre cas su: 0,1,2,3,...
// pre lfBallDecayInTime = pow(ball_decay, time (cas 0 je specialny pripad)
bool InterceptBallBehaviour::IsBallInPlayerKickableAreaDuringInteception(double lfBallDecayInTime, int time)
{
	// Toto je hracov dosah, kedy este moze kopnut do lopty	
	const double lfPlayerRadius = world->params->server->ball_size + 
		world->me->type->player_size + world->me->type->kickable_margin * 0.51;
	
	// Specialny pripad, neobsahuje geometrice rady
	if (time == 0 && lfPlayerRadius >= 0)
		return (world->ball->pos - world->me->pos).GetLength() <= (lfPlayerRadius * lfPlayerRadius);
	
	// Tento vektor je vektor od hraca (v case 0) k lopte v case "time"
	Vector vectFromPlayerToBall = world->ball->pos + (world->ball->vel * 
		(1.0f - (float)lfBallDecayInTime)/(1.0f - world->params->server->ball_decay)) - world->me->pos;

	// Optimisticky odhad
//	double lfPlayerCircleRadius = time * world->me->type->player_speed_max + lfPlayerRadius;
	
	// Realnejsi odhad
	double lfPlayerCircleRadius = time * (world->me->vel).GetLength() + lfPlayerRadius;

  return (vectFromPlayerToBall.GetLength() <= lfPlayerCircleRadius);
}


// Bezi na zadanu poziciu, na ktorej ocakava najvhodnejsie zachytenie lopty 
void InterceptBallBehaviour::RunToBallInteceptPosition(Point position)
{
	int nsteps = -1;
	int psteps;
	Angle uhol;
	Angle uholT = DegAngle(0);
	Ball tmpBall(*(world)->ball);
	tmpBall.SetLastSeenPosition(position);
	Vector diff;

	float dashpower;


	// Bezi sa co narychlejsie, ak zachytavame prihravku hlboko v obrane supera alebo nasej obrane
	if(IsPointInRectArea(world->ball->pos.GetX(), world->ball->pos.GetY(), 28.0, -34.0, 52.5, 34.0) ||
		IsPointInRectArea(position.GetX(), position.GetY(), -52.5, -34.0, -25.0, 34.0))
	{
		
#ifdef GOALIE
		if(IsPointInRectArea(position.GetX(), position.GetY(), -52.5, -20.0, -27, 20.0))
		{	
			DBGN("\n[Cyklus %d] Pouzivam zrychlene zachytavanie lopty na pozicii: (%lf, %lf)", world->state->GetSimTime(), world->ball->pos.GetX(), world->ball->pos.GetY());
			dashpower = GetRunToPositionBehaviour()->RecommendDashToMaxSpeed();
		}
		else
			dashpower = GetRunToPositionBehaviour()->RecommDashPower(true);
#else	// GOALIE
		if(ShouldInterceptBall())
			dashpower = GetRunToPositionBehaviour()->RecommendDashToMaxSpeed();
		else
			dashpower = GetRunToPositionBehaviour()->RecommDashPower(false);
#endif	// GOALIE
	
	}
	else
		dashpower = GetRunToPositionBehaviour()->RecommDashPower(true);

	if((world->ball->pos - world->me->pos).GetLength() < 6.0)
		dashpower = GetRunToPositionBehaviour()->RecommendDashToMaxSpeed();

	do
	{
		diff = tmpBall.pos - world->me->pos;
		uhol = CalcDir(diff, world->me->GetAbsDir());
		
		if(uhol.GetAbsDeg() > 15)
		{
			diff = tmpBall.pos - (world->me->pos + GetRunToPositionBehaviour()->PredictNextVelocity(dashpower));
			uholT = CalcDir(diff, world->me->GetAbsDir());
			psteps = 1 + GetRunToPositionBehaviour()->PredictSteps(diff.GetLength(), dashpower);
		}
		else
		{
			psteps = GetRunToPositionBehaviour()->PredictSteps(diff.GetLength(), dashpower, 
				GetRunToPositionBehaviour()->PredictNextVelocity(dashpower).GetLength());
		}
		
		nsteps++;
		tmpBall.Step();
	}
	while(nsteps < psteps);

	Ball tmpBall2(*(world)->ball);	
	tmpBall2.Step();

	if(uhol.GetAbsDeg() > 15)
	{
		float TurnMoment = GetRunToPositionBehaviour()->CompMoment(uholT.GetDeg());
		commandGenerator->Turn(TurnMoment);
		GetTurnNeckToBehaviour()->Behave(tmpBall2.pos, world->me->pos + world->me->vel, 
			GetTurnToBehaviour()->CompTurn(TurnMoment));
	}
	else
	{
		commandGenerator->Dash(dashpower);
		GetTurnNeckToBehaviour()->Behave(tmpBall2.pos, world->me->pos + GetRunToPositionBehaviour()->PredictNextVelocity(dashpower), 0);
	}	
}


// Mal by hrac vobec uvazovat nad zachytenim lopty?
bool InterceptBallBehaviour::ShouldInterceptBall()
{
	if(world->state->GetPlayMode() != PM_PlayOn)
	{
		if(DATA_STORAGE.ShouldKickIn())
			return true;
		else
			return false;
	}

	return true;
}

/* 
  Vrati bod najblizsie k aktualnej pozicii brankara, kde moze chytit loptu, alebo aspon kopnut do lopty.
  Vrati sa najpravdepodobnejsi odchytavaci bod.
  Nie je mozne chytit/kopnut do lopty, ak navratova hodnota ma x-ovu zlozku = -1000
 */
// PRAVDEPODOBNE NIE JE TATO FUNKCIA 100% FUNKCNA!
Point InterceptBallBehaviour::GetNearestBallInterceptPos()
{
	const int MAX_CYCLE = 30;

	int cycle, cycleb, minc;
	float catchableArea, kickableArea, actArea, edp, deg;
	Point res, T1, T2;
	Vector a, v;
	
	struct FBS
	{
		Point p;
		bool in;
	} futureBallPos[MAX_CYCLE];
	
	Point futureMePos[MAX_CYCLE];
	float runDist[MAX_CYCLE];
	int maxCycle;

	Myself p(*world->me);
	Ball tmpBall(*world->ball);

	catchableArea = max(world->params->server->catchable_area_l, world->params->server->catchable_area_w);
	kickableArea = world->me->type->player_size + world->params->server->ball_size + world->me->type->kickable_margin;
	edp = p.GetEffort() * world->me->type->dash_power_rate * 100;
	
	if(edp > world->params->server->player_accel_max)
		edp = world->params->server->player_accel_max;

	deg = fabsf(p.GetAbsDir().GetDeg() - world->ball->vel.GetAngle().GetDeg());
	
	if(deg > 180)
		deg = 360 - deg;
	if(deg > 90)
		deg = 180; // utekame spatne
	else
		deg = 0;

	futureBallPos[0].p = world->ball->pos;
	futureMePos[0] = p.pos;
	runDist[0] = 0;
	v.SetLength(0);
	
	for(cycleb = 1; cycleb < MAX_CYCLE; cycleb++)
	{
		// bez otacania
		a = PolarVector(edp, p.GetAbsDir() + DegAngle(deg));
		p.vel += a;
		
		if(p.vel.GetLength() > world->me->type->player_speed_max)
			p.vel.SetLength(world->me->type->player_speed_max);
		
		futureMePos[cycleb] = futureMePos[cycleb - 1] + p.vel;
		p.vel *= world->me->type->player_decay;

		// s otocenim
		if(cycleb > 1) 
		{
			v.SetLength(v.GetLength() + edp);
			
			if(v.GetLength() > world->me->type->player_speed_max)
				v.SetLength(world->me->type->player_speed_max);
		}
		runDist[cycleb] = runDist[cycleb - 1] + v.GetLength();
		
		if(cycleb > 1)
			v.SetLength(v.GetLength() * world->me->type->player_decay);

		// lopta
		tmpBall.Step();
		futureBallPos[cycleb].p = tmpBall.pos;
		futureBallPos[cycleb].in = DATA_STORAGE.InPenaltyArea(tmpBall.pos);
		
		if(DATA_STORAGE.goalieCatch.goalieCatchBan > 0 && cycleb <= DATA_STORAGE.goalieCatch.goalieCatchBan) 
		{
			futureBallPos[cycleb].in = false;
		}
		else if(fabsf(tmpBall.pos.GetX()) > 52.5)
			break;
	}
	maxCycle = cycleb;

	minc = -1;
	res.SetX(-1000);
	p = *world->me;
	// Presuvame sa momentom zatial co sa otacame 
	p.pos = p.pos + p.vel;

	// Znizujeme pravdepodobnost
	for(float per = .3f; per <= 1.1; per += .1f)
	{
		kickableArea *= per;
		catchableArea *= per;

		for(cycleb = 0; cycleb < maxCycle; cycleb++)
		{
			if(minc != -1 && cycleb > minc)
				break;

			actArea = futureBallPos[cycleb].in ? catchableArea : kickableArea;

			// Najskor skus bez otacania
			for(cycle = cycleb; cycle >= 0; cycle--)
			{
				if((futureBallPos[cycleb].p - futureMePos[cycle]).GetLength() <= actArea)
				{
					if((cycleb <= minc && per < .8) || minc == -1) {
						res = futureMePos[cycle];
						if(fabsf(res.GetX()) > 52.5) res.SetX(-1000);
						else
						{
							minc = cycleb;
	//						DBGN("%.1f ", per);
						}
					}
				}
			}

			// Ak sa nic nenaslo, tak skus s otacanim
			for(cycle = cycleb; cycle >= 0 && minc == -1; cycle--)
			{
				if((futureBallPos[cycleb].p - p.pos).GetLength() <= runDist[cycle] + actArea)
				{
					if((cycleb < minc && per < .8) || minc == -1) {
						res = futureBallPos[cycleb].p;
						
						if(fabsf(res.GetX()) > 52.5)
							res.SetX(-1000);
						else
						{
							minc = cycleb;
//							DBGN("%.1f ", per);
						}
					}
					break;
				}
			}
		}

		kickableArea /= per;
		catchableArea /= per;
	}

	if(minc == 0 && (world->ball->GetAge() == 0 || world->state->GetPlayMode() == PM_GoalKick_Our))
		res = futureMePos[2];

	return res;
}
